// Meena Abdou + Chris Bena
// CPEG 467 - ERA MOBI Robot project
// 11/30/2010

/*
 * Drive around by hugging walls.  Should be able to traverse a maze.
 * Hugs the right wall.
 * 
 * This program DOES NOT use functions for turning 90 degrees and corners.
 * Instead it continues going straight whilst turning to make a nice smooth turn.
 *
 * We are now trying to make a class that would solve our odometry problems.
 *
 */

#include <libplayerc++/playerc++.h>
#include <iostream>
#include <fstream>

//#include "args.h"
#include "customPosition.h"

#define FRONT 327
#define RIGHT 110
#define LEFT 546
#define MIN_RANGE 0.5 // min range on the right
#define MAX_RANGE 1.0 // max range on the right
#define COLLISION 0.35 // the min range for collision detection
#define CONS3 0.01 // constant for test 3
#define PI 3.14159265

// max range set to 1.5 for stage; on robot it 
// should be about 0.8-1.0

using namespace PlayerCc;

PlayerClient robot(gHostname, gPort);
Position2dProxy pp(&robot, gIndex);
LaserProxy lp(&robot, gIndex);
//std::ofstream fout;
std::ofstream fout2;

void turn(double newspeed, double newturnrate){
  pp.SetSpeed(newspeed, newturnrate);
}

// Turns right - turns until lp[FRONT] is maxed out;
int turn(){
  int count=0;
  do{
    pp.SetSpeed(0.0, 0.375);
    robot.Read();
    count++;
  }
  while(lp[FRONT] < 5.58);
return count;
}

//void saveData(){
// double x = pp.GetXPos();
//double y = pp.GetYPos();
//double yaw = pp.GetYaw(); 
//fout << x
// << " "
//     << y
//     << " "
//     << rtod(yaw)
//     << " "
//     << lp[RIGHT]
//     << " "
//     << lp[LEFT] << "\n";
//}

void saveData2(double x, double y, double yaw){ 
  fout2  << x 
	 << " "
	 << y
	 << " "
	 << rtod(yaw)
	 << " "
	 << lp[RIGHT]
	 << " "
	 << lp[LEFT] << "\n";
}


int main(int argc, char **argv)
{
  
  parse_args(argc,argv);
  //fout.open("OutputData.txt");
  //if(!fout.is_open()){
  //std::cerr << "Error opening file\n";
  //exit(-1);
  //}
  
  fout2.open("OutputDataCustom1.txt");
  if(!fout2.is_open()){
    std::cerr << "Error opening file 2\n";
    exit(-1);
  }
  //fout.setf(std::ios::fixed, std::ios::floatfield);
  //fout.precision(8);
  
  fout2.setf(std::ios::fixed, std::ios::floatfield);
  fout2.precision(8);;
  // we throw exceptions on creation if we fail
  try
    {
      pp.SetMotorEnable (true);
      double newspeed = 0;
      double newturnrate = 0;
      int counter=0;
      double x=0;
      double y=0;
      double yaw=0;
      Position pos1;
      Position pos2;
      Position pos3;
      Position pos4;
      robot.Read(); //here so GetRange doesn't segfaultt
    // go into read-think-act loop
      for(;;)
	{
	  counter=0;
	  // this blocks until new data comes; 10Hz by default
	  robot.Read();
	  /* Using only the certain sensors - front, left, right */
	  std::cout<<"0" << std::endl;
	  //double front=5.6;
	  double front = lp[FRONT];    // Seg fault
	  std::cout<<"1" << std::endl;
	  double frontRight = lp[218];
	  std::cout<<"2" << std::endl;
	  
	  double frontLeft = lp[437];
	  std::cout<<"3" << std::endl;
	  double right = lp[RIGHT];
	  std::cout<<"4" << std::endl;
	  double left = lp[LEFT];
	  std::cout<<"5" << std::endl;
	  double yaw = pp.GetYaw();
	  std::cout<<"6" << std::endl;
	  double minRight = lp.GetMinRight();
	  std::cout<<"7" << std::endl;
	  double minDistRight=5.6;
	  double minDistLeft=5.6;
	  double changedir=0.0;
	  
	  std::cout<<"8" << std::endl;
	  for(int i = RIGHT; i <= FRONT; i++){
	    if(lp[i] < minDistRight)
	      minDistRight=lp[i];
	  }
	  for(int i = FRONT; i <= LEFT; i++){
	    if(lp[i] < minDistLeft)
	      minDistLeft=lp[i];
	  }
	  
	  // if inside range
	  if(frontRight <= MIN_RANGE || minDistRight <= COLLISION){
	    newspeed = 0.0;
	    newturnrate = 0.175;
	    turn(newspeed, newturnrate);
	  }
	  
	  // else if outside range
	  else if(frontRight > MAX_RANGE || minDistLeft <= COLLISION){
	    newspeed = 0.25;
	    newturnrate = -0.190;
	    turn(newspeed, newturnrate);
	  }
	  
	  else{
	    newspeed = 0.5;
	    newturnrate = 0.0;
	    turn(newspeed, newturnrate);
	  }
	  std::cout<<"9" << std::endl;
	  // if reached a corner
	  if(front <= 1.5 && frontRight <= 1.3){
	    
	    turn();
	    //do{
	    //newspeed=0.25;
	    //newturnrate=0.375;
	    //pp.SetSpeed(0.25, 0.375);
	    //robot.Read();
	    //}
	    //while(lp[FRONT] < 5.58);
	  }
	  
	  // when we test for front <= 1.0 it gets very close to the wall 
	  // and then starts to turn.  The reason is that there's a while
	  // between when the condition turns true and the robot responds
	  // to the new turn command.
	  
	  yaw=pp.GetYaw();
	  x=pp.GetXPos();
	  y=pp.GetYPos();
	  pos1.update(yaw,x,y);
	  
	  //saveData();
	  saveData2(pos1.getX(), pos1.getY(), pos1.getYaw());
	}
    }
  
  catch (PlayerCc::PlayerError e)
    {
      std::cerr << e << std::endl;
      return -1;
    }
}
